﻿#include "dome_pid.h"
#include "pid.h"

DomePid::DomePid()
{
    // m_pointX = 0.5;
    // m_pointY = 0.5;
    // m_pointZ = 0.2;

    m_pointX = -0.438055992;
    m_pointY = 0.0423640013;
    m_pointZ = 0.240881994;

    m_pidX = std::make_shared<PID>(&m_domeInX, &m_domeOutSpeedX, &m_pointX, 0, 0, 0, 0, 0);
    m_pidY = std::make_shared<PID>(&m_domeInY, &m_domeOutSpeedY, &m_pointY, 0, 0, 0, 0, 0);
    m_pidZ = std::make_shared<PID>(&m_domeInZ, &m_domeOutSpeedZ, &m_pointZ, 0, 0, 0, 0, 0);

    m_pidX->SetOutputLimits(-1, 1);
    m_pidY->SetOutputLimits(-1, 1);
    m_pidZ->SetOutputLimits(0, 1);

    m_pidX->SetControllerDirection(0);
    m_pidY->SetControllerDirection(0);
    m_pidZ->SetControllerDirection(0);

    m_pidX->SetSampleTime(10);
    m_pidY->SetSampleTime(10);
    m_pidZ->SetSampleTime(10);

    m_pidX->SetTunings(30.1, 0.12, 0.4, 1);
    m_pidY->SetTunings(22.1, 0.12, 0.4, 1);
    m_pidZ->SetTunings(3.2, 0.12, 0.4, 1);


    m_pidX->SetMode(1);
    m_pidY->SetMode(1);
    m_pidZ->SetMode(1);
}

DomePid::~DomePid()
{
}

void DomePid::compute()
{
    m_pidX->Compute();
    m_pidY->Compute();
    m_pidZ->Compute();
}